/*!
  @file solver_method_arun.hpp
  @copyright 2013 Kubota Lab. All rights resereved.
*/

#ifndef _SOLVER_METHOD_ARUN_HPP_
#define _SOLVER_METHOD_ARUN_HPP_

#include "mrover_vo/solver_method_arun.h"

#include <ros/console.h>

namespace vo {

template <typename ValType>
void SolverMethodArun<ValType>::initializeData(const std::vector<int> &samples) {
  if (!initCompute()) {
    ROS_WARN("Error in SolverMethodArun");
    return;
  }

  // Y = iTt * X   (i.e., X = target, Y = input)
  num_sample_ = samples.size();
  X.resize(3, num_sample_);
  Y.resize(3, num_sample_);
  for (size_t i = 0; i < num_sample_; ++i) {
    cv::Point3f _pt_in = input_ ->sp->at(matches_->at(samples[i]).queryIdx);
    cv::Point3f _pt_tr = target_->sp->at(matches_->at(samples[i]).trainIdx);
    X.col(i) << _pt_tr.x, _pt_tr.y, _pt_tr.z;
    Y.col(i) << _pt_in.x, _pt_in.y, _pt_in.z;
  }
}

template <typename ValType>
void SolverMethodArun<ValType>::computePose() {
  Affine3T iTt;

  Vector3T uX = X.rowwise().mean();
  Vector3T uY = Y.rowwise().mean();

  MatrixXT dX = X - uX.rowwise().replicate(num_sample_);
  MatrixXT dY = Y - uY.rowwise().replicate(num_sample_);

  // TODO check collinearity

  MatrixXT H = dX * dY.transpose();
  JacobiSVD<MatrixXT> svd(H, ComputeThinU | ComputeThinV);
  iTt.linear() = svd.matrixV() * svd.matrixU().transpose();
  if (iTt.linear().determinant() < 0) {
    //ROS_WARN("Error in computing rotation matrix (Arun)");
    return;
  }

  iTt.translation() = uY - iTt.linear() * uX;

  poses_->push_back(iTt);
}

}  // namespace vo

#endif  // _SOLVER_METHOD_ARUN_HPP_
